'''
Author: Zhuangyu
Date: 2022-06-02 14:31:11
LastEditTime: 2022-11-25 22:46:28
'''
from __future__ import print_function, division
import time
#Importing relevant modules and packages
import numpy as np
import matplotlib.pyplot as plt
import random
from Parameters import *
from scipy import integrate, linalg, interpolate
from tqdm import tqdm
from casadi import *
import mpctools as mpc
import casadi
import scipy.io as sio
from ode_sensitivity_States import * 
import van_Genuchten as vg
from RichardsModel_1D import *
from prejacobian import F1,F2,F3,Fy
from ode_sensitivity import *

#Space and time parameters
Nr,Nt,Nz,dr,dt,dz,Np,Na,Np,N_obs,Hz=circular_parameters()
DeltaT,Nsim=time_parameters()
p=Loam() #Soil parameters

size=Nsim

# u=np.zeros(20*120)
# uu=np.zeros(Nsim)
# for i in range(20*120):
#     if i < 55:
#         u[i]=-1e-06 #Moisture is supplied only at the first time instant
#     else:
#         u[i]=0
# uu=np.tile(u,(1,60))
# uu=np.transpose(uu)

u=np.zeros(4*120)
uu=np.zeros(Nsim)
for i in range(4*120):
    if i < 8:
        u[i]=-1e-06 #Moisture is supplied only at the first time instant
    else:
        u[i]=0
uu=np.tile(u,(1,60))
uu=np.transpose(uu)


#Initial condition [pressure head]
x0=-1.1*np.ones(Nz)

#Numpy array to store the simulation results [pressure head]
xx=np.zeros((Nsim+1,Nz))
xx[0,:]=x0

ode1=np.zeros((Nsim+1,Nz))

#Model uncertainty
# www1 = 4e-9 # process noise
# vv1 = 8e-7*np.ones(N_obs) # measurement noise
www1 = 4e-9 # process noise
vv1 = 8e-7*np.ones(N_obs) # measurement noise

np.random.seed(2)
w=np.random.normal(0,np.sqrt(www1),[Nsim+1,Nz])
# w=0.01*np.random.randn(Nsim,Nz)
np.random.seed(3)
vv=np.random.randn(Nsim+1,N_obs)*vv1 #measurement noise

#Numpy array to store the simulation results [volumetric moisture content]
yy=np.zeros((Nsim+1,N_obs))
yy[0,:]=getOutputs1D(x0).full().ravel()+vv[0,:]


#unknown inputs
a1=0*np.ones((Nsim+1,Nz))
a=0.3*np.ones((Nsim+1,Nz))

e0 = time.time()

for i in tqdm(range(1, Nsim+1)):
    # Generate observed data (x bias)
    xx[i,:]= DM(ODE_discrete(tuple(xx[i-1]), uu[i-1],a[i-1],DeltaT)).full().ravel()+w[i-1]    
    yy[i,:]=getOutputs1D(xx[i,:]).full().ravel()+vv[i,:]
    

# t= list(range(Nsim+1))
# plt.figure(1)
# for i in range (Nz):
#     plt.subplot(4, 4, i+1)
#     plt.plot(t,xx[:,i],'r')
# plt.show()

Sall1=ode_sensitivity(xx,uu,a)
np.savetxt('Sall.txt',Sall1)

U,Sigma,VT = np.linalg.svd(Sall1)
# Slog = np.log10(Sigma)
# print('log(Sigma)=',  Slog)

rank = RANK(np.diag(Sigma))
# rank_S = rank
# cond = np.linalg.cond(Sigma)
print("rank:",rank)
# print("cond:",cond)
